#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// OLED屏幕配置
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
#define SDA_PIN 27
#define SCL_PIN 26
TwoWire I2Cone = TwoWire(0);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &I2Cone, OLED_RESET);

// 输入设备引脚定义
#define THROTTLE_PIN 34
#define JOYSTICK_X_PIN 35
#define JOYSTICK_Y_PIN 32
#define POTENTIOMETER_PIN 33
#define THROTTLE_SWITCH_PIN 25
#define JOYSTICK_BUTTON_PIN 14  // 摇杆按钮用于翻页

// 页面管理
#define MAX_PAGES 3
int currentPage = 0;

// 传感器数据
int throttleValue = 0;
int joystickX = 0;
int joystickY = 0;
int potentiometerValue = 0;
int throttleSwitchState = 0;
String joystickDirection = "CENTER";

// 按钮状态变量
int buttonState;
int lastButtonState = HIGH;
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50;

void setup() {
  Serial.begin(115200);
  
  // 初始化自定义I2C引脚
  I2Cone.begin(SDA_PIN, SCL_PIN, 400000);
  
  // 初始化OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 display allocation failed"));
    for(;;);
  }
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  
  // 设置输入引脚
  pinMode(THROTTLE_SWITCH_PIN, INPUT_PULLUP);
  pinMode(JOYSTICK_BUTTON_PIN, INPUT_PULLUP);
  
  // 显示初始页面
  displayPage(currentPage);
}

void loop() {
  // 读取传感器值
  readSensors();
  
  // 处理传感器数据
  processSensorData();
  
  // 检测页面切换
  checkPageChange();
  
  // 在OLED上显示当前页面
  displayPage(currentPage);
  
  delay(100); // 控制刷新频率
}

void readSensors() {
  throttleValue = analogRead(THROTTLE_PIN);
  joystickX = analogRead(JOYSTICK_X_PIN);
  joystickY = analogRead(JOYSTICK_Y_PIN);
  potentiometerValue = analogRead(POTENTIOMETER_PIN);
  throttleSwitchState = digitalRead(THROTTLE_SWITCH_PIN);
}

void processSensorData() {
  // 确定摇杆方向
  joystickDirection = "CENTER";
  if (joystickX < 300) joystickDirection = "LEFT";
  if (joystickX > 3900) joystickDirection = "RIGHT";
  if (joystickY < 300) joystickDirection = "UP";
  if (joystickY > 3900) joystickDirection = "DOWN";
  
  // 将电位器值转换为-100到100的范围
  potentiometerValue = map(potentiometerValue, 0, 4095, -100, 100);
}

void checkPageChange() {
  // 读取摇杆按钮状态并进行消抖处理
  int reading = digitalRead(JOYSTICK_BUTTON_PIN);
  
  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }
  
  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading != buttonState) {
      buttonState = reading;
      
      // 如果按钮被按下(低电平)，切换到下一页
      if (buttonState == LOW) {
        currentPage = (currentPage + 1) % MAX_PAGES;
      }
    }
  }
  
  lastButtonState = reading;
}

void displayPage(int page) {
  display.clearDisplay();
  
  switch(page) {
    case 0:
      displayMainPage();
      break;
      
    case 1:
      displaySensorPage();
      break;
      
    case 2:
      displayControlPage();
      break;
  }
  
  // 显示页脚信息
  display.setCursor(0, 58);
  display.setTextSize(1);
  display.print("Page ");
  display.print(currentPage + 1);
  display.print("/");
  display.print(MAX_PAGES);
  display.setCursor(100, 58);
  display.print("Press >");
  
  display.display();
}

void displayMainPage() {
  display.setTextSize(2);
  display.setCursor(0, 0);
  display.println("Control Panel");
  display.setTextSize(1);
  
  display.drawLine(0, 15, 128, 15, SSD1306_WHITE);
  
  display.setCursor(10, 25);
  display.println("System Status: READY");
  
  display.setCursor(10, 40);
  display.print("Throttle: ");
  display.println(throttleSwitchState == LOW ? "ON" : "OFF");
  
  display.setCursor(10, 50);
  display.print("Potentiometer: ");
  display.print(potentiometerValue);
}

void displaySensorPage() {
  display.setTextSize(2);
  display.setCursor(0, 0);
  display.println("Sensor Data");
  display.setTextSize(1);
  
  display.drawLine(0, 15, 128, 15, SSD1306_WHITE);
  
  display.setCursor(10, 22);
  display.print("Throttle: ");
  display.println(throttleValue);
  
  display.setCursor(10, 32);
  display.print("Joystick X: ");
  display.println(joystickX);
  
  display.setCursor(10, 42);
  display.print("Joystick Y: ");
  display.println(joystickY);
  
  display.setCursor(10, 52);
  display.print("Potentiometer: ");
  display.println(potentiometerValue);
}

void displayControlPage() {
  display.setTextSize(2);
  display.setCursor(0, 0);
  display.println("Control View");
  display.setTextSize(1);
  
  display.drawLine(0, 15, 128, 15, SSD1306_WHITE);
  
  // 显示摇杆方向（箭头）
  display.setCursor(10, 22);
  display.print("Joystick: ");
  
  int arrowX = 64;  // 箭头中心点X坐标
  int arrowY = 30;  // 箭头中心点Y坐标
  int arrowSize = 10;  // 箭头大小
  
  if (joystickDirection == "UP") {
    display.drawLine(arrowX, arrowY - arrowSize, arrowX, arrowY + arrowSize, SSD1306_WHITE);
    display.drawLine(arrowX, arrowY - arrowSize, arrowX - arrowSize/2, arrowY - arrowSize/2, SSD1306_WHITE);
    display.drawLine(arrowX, arrowY - arrowSize, arrowX + arrowSize/2, arrowY - arrowSize/2, SSD1306_WHITE);
  } else if (joystickDirection == "DOWN") {
    display.drawLine(arrowX, arrowY - arrowSize, arrowX, arrowY + arrowSize, SSD1306_WHITE);
    display.drawLine(arrowX, arrowY + arrowSize, arrowX - arrowSize/2, arrowY + arrowSize/2, SSD1306_WHITE);
    display.drawLine(arrowX, arrowY + arrowSize, arrowX + arrowSize/2, arrowY + arrowSize/2, SSD1306_WHITE);
  } else if (joystickDirection == "LEFT") {
    display.drawLine(arrowX - arrowSize, arrowY, arrowX + arrowSize, arrowY, SSD1306_WHITE);
    display.drawLine(arrowX - arrowSize, arrowY, arrowX - arrowSize/2, arrowY - arrowSize/2, SSD1306_WHITE);
    display.drawLine(arrowX - arrowSize, arrowY, arrowX - arrowSize/2, arrowY + arrowSize/2, SSD1306_WHITE);
  } else if (joystickDirection == "RIGHT") {
    display.drawLine(arrowX - arrowSize, arrowY, arrowX + arrowSize, arrowY, SSD1306_WHITE);
    display.drawLine(arrowX + arrowSize, arrowY, arrowX + arrowSize/2, arrowY - arrowSize/2, SSD1306_WHITE);
    display.drawLine(arrowX + arrowSize, arrowY, arrowX + arrowSize/2, arrowY + arrowSize/2, SSD1306_WHITE);
  } else {
    display.drawCircle(arrowX, arrowY, arrowSize/2, SSD1306_WHITE);
  }
  
  // 显示电位器值（能量条+数字）
  display.setCursor(10, 45);
  display.print("Potentiometer: ");
  
  // 绘制能量条背景
  display.drawRect(100, 45, 20, 8, SSD1306_WHITE);
  
  // 计算能量条高度（根据电位器值-100到100）
  int barHeight = map(potentiometerValue, -100, 100, 0, 8);
  
  // 绘制能量条
  display.fillRect(101, 53 - barHeight, 18, barHeight, SSD1306_WHITE);
  
  // 显示电位器数字值
  display.setCursor(70, 45);
  display.print(potentiometerValue);
  
  // 显示油门开关状态
  display.setCursor(10, 55);
  display.print("Throttle: ");
  display.println(throttleSwitchState == LOW ? "ON" : "OFF");
}